#include "Physics.h"
#include <math.h>
#include "Player.h"
#include "Game.h"

#define Fullscreen 1.67f

void OrbitClockwise(Entity * invokingObject, Entity * orbitCenter, float orbitSpeed)
{
	SGD::Vector rotate = orbitCenter->GetRect().ComputeCenter() - invokingObject->GetRect().ComputeCenter();
	/*rotate.Normalize();
	rotate *= orbitSpeed;
	if (invokingObject->GetType() == Entity::ENT_PLAYER)
	{
		Player * player = dynamic_cast<Player *>(invokingObject);
		if (player->GetDestinationPlanet() != nullptr && player->GetCurrentPlanet() == nullptr)
		{
			rotate *= player->GetOrbitSpeed() / orbitSpeed;
		}

	}
	rotate.Rotate(-1.53f);
	invokingObject->SetVelocity(rotate);*/
	SGD::Vector velocity{ sinf(invokingObject->GetRotation()) /** (rotate.ComputeLength() - invokingObject->GetRadius())*/, -cosf(invokingObject->GetRotation())/* * (rotate.ComputeLength() - invokingObject->GetRadius())*/ };
	if (Game::GetInstance()->GetWindowed())
	velocity.Rotate(1.602f /*1.59f*/);
	else
		velocity.Rotate(Fullscreen);
	velocity *= 300;
	if (invokingObject->GetType() == Entity::ENT_PLAYER)
	{
		Player * player = dynamic_cast<Player *>(invokingObject);
		if (player->GetDestinationPlanet() != nullptr && player->GetCurrentPlanet() == nullptr)
		{
			rotate *= player->GetOrbitSpeed() / 300;
		}

	}
	invokingObject->SetVelocity(velocity);
}

void OrbitCounterClockwise(Entity * invokingObject, Entity * orbitCenter, float orbitSpeed)
{
	SGD::Vector rotate = orbitCenter->GetRect().ComputeCenter() - invokingObject->GetRect().ComputeCenter();
	/*rotate.Normalize();
	rotate *= orbitSpeed;
	if (invokingObject->GetType() == Entity::ENT_PLAYER)
	{
		Player * player = dynamic_cast<Player *>(invokingObject);
		if (player->GetDestinationPlanet() != nullptr && player->GetCurrentPlanet() == nullptr)
		{
			rotate *= -player->GetOrbitSpeed() / orbitSpeed;
		}
	}
	rotate.Rotate(1.53f);
	invokingObject->SetVelocity(rotate);
	*/
	SGD::Vector velocity{ sinf(invokingObject->GetRotation()) /** (rotate.ComputeLength() - invokingObject->GetRadius())*/, -cosf(invokingObject->GetRotation()) /** (rotate.ComputeLength() - invokingObject->GetRadius())*/ };
	if (Game::GetInstance()->GetWindowed())
	velocity.Rotate(-1.602f/*-1.59f*/);
	else
	velocity.Rotate(-Fullscreen);

	velocity *= 300;
	if (invokingObject->GetType() == Entity::ENT_PLAYER)
	{
		Player * player = dynamic_cast<Player *>(invokingObject);
		if (player->GetDestinationPlanet() != nullptr && player->GetCurrentPlanet() == nullptr)
		{
			rotate *= -player->GetOrbitSpeed() / 300;
		}

	}
	invokingObject->SetVelocity(velocity);
	/*for (int theta = 0; theta<2 * SGD::PI; theta += 0.01)
	{
		float x = r*cos(theta),
			y = r*sin(theta);
		earth = vec2(x, y);
	}
	SGD::Vector rotate = orbitCenter->GetRect().ComputeCenter() - invokingObject->GetRect().ComputeCenter();
	float radius = rotate.ComputeLength();
	float angle = acosf(rotate.y / radius);
	if (rotate.y < 0)
	{
		rotate.x = radius * cos(angle);
	}
	else
	{
		rotate.x = -radius * cos(angle);
	}
	rotate.y = radius * sin(angle);

	//rotate.Normalize();
	//rotate *= 300;
	rotate.Rotate(angle);
	invokingObject->SetVelocity(rotate);
	*/
}

void PhysicsUpdate(Entity * invokingObject, float elapsedTime)
{
	invokingObject->SetAcceleration(invokingObject->GetForce() / invokingObject->GetMass());
	invokingObject->SetVelocity(invokingObject->GetVelocity() + invokingObject->GetAcceleration() * elapsedTime);
	invokingObject->SetPosition(invokingObject->GetPosition() + invokingObject->GetVelocity() * elapsedTime);
	DiminishForce(invokingObject);
}

void ApplyForce(Entity * invokingObject, SGD::Vector inForce)
{
	invokingObject->SetForce(invokingObject->GetForce() + inForce);
}

void DiminishForce(Entity * invokingObject)
{
	invokingObject->SetForce(invokingObject->GetForce() * .99f);
	if (invokingObject->GetForce().ComputeLength() < .01f)
	{
		ZeroForce(invokingObject);
	}
}

void ZeroForce(Entity* invokingObject)
{
	invokingObject->SetForce(SGD::Vector{ 0, 0 });
}

void RotateForce(Entity * invokingObject, float radians)
{
	invokingObject->SetForce(invokingObject->GetForce().ComputeRotated(radians));
}

void Jump(Entity * invokingObject, Entity * surfaceObject, float speed)
{
	SGD::Vector direction = invokingObject->GetRect().ComputeCenter() - surfaceObject->GetRect().ComputeCenter();
	direction.Normalize();
	direction *= speed;
	invokingObject->SetVelocity(invokingObject->GetVelocity() + direction);
}


void ComputeCentripetalForce(Entity* invokingObject, Entity * orbitCenter)
{
	SGD::Vector NewForce;
	NewForce.x = invokingObject->GetVelocity().x * invokingObject->GetVelocity().x;
	NewForce.y = invokingObject->GetVelocity().y * invokingObject->GetVelocity().y;
	NewForce *= invokingObject->GetMass();
	
	SGD::Vector length = invokingObject->GetPosition() - orbitCenter->GetPosition();
	NewForce /= length.ComputeLength();
	invokingObject->SetForce(NewForce);
}

void PhysicsCollision(Entity * InvokingObject, Entity * CollidedObject)
{
	SGD::Vector Force = InvokingObject->GetVelocity() * InvokingObject->GetMass() + CollidedObject->GetVelocity() * CollidedObject->GetMass();
	InvokingObject->SetVelocity(Force / InvokingObject->GetMass());
	CollidedObject->SetVelocity(-Force / CollidedObject->GetMass());
	float damage = Force.ComputeLength();
	if (CollidedObject->GetType() == Entity::ENT_ENEMY)
	{


	}
	if (CollidedObject->GetType() == Entity::ENT_PLAYER)
	{
		Player * player = dynamic_cast<Player*>(CollidedObject);
		player->SetHealth(player->GetHealth() - damage / player->GetMass());
	}
}
